// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
//-
// Copyright (c) 2010 Michael Smith. All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
// SUCH DAMAGE.

/// @file       decoder.pde
/// @brief      ArduPilotMega binary protocol decoder

void
Decoder::poll(void)
{
        uint8_t avail;

        avail = Serial.available();
        if (0 == avail) {
                // have we been waiting too long for data?
                if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT)
                        _phase = DEC_WAIT_P1;
        } else {
                // new data
                while (avail--)
                        _kick(Serial.read());
                _lastReceived = millis();
        }
}

void
Decoder::_kick(uint8_t inByte)
{
        uint8_t         tableIndex;

        switch (_phase) {
                // Preamble detection
                //
                // Note the fallthrough from P2 to P1 deals with the case where
                // we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or 
                // a SUM_B byte we never looked at.
        case DEC_WAIT_P2:
                if ('D' == inByte) {
                        _phase++;

                        // prepare for the header
                        _bytesIn = 0;
                        _bytesExpected = sizeof(struct msg_packet_header);

                        // intialise the checksum accumulators
                        _sumA = _sumB = 0;

                        break;
                }
                _phase = DEC_WAIT_P1;
                // FALLTHROUGH
        case DEC_WAIT_P1:
                if ('4' == inByte) {
                        _phase++;
                }
                break;

                // receiving the header
        case DEC_WAIT_HEADER:
                // do checksum accumulation
                _sumA += inByte;
                _sumB += _sumA;

                // store the byte
                _buf.bytes[_bytesIn++] = inByte;

                // check for complete header received
                if (_bytesIn == _bytesExpected) {
                        _phase++;

                        // prepare for the payload
                        // variable-length data?
                        _bytesIn = 0;
                        _bytesExpected = _buf.header.length;
                        _messageID = _buf.header.messageID;
                        _messageVersion = _buf.header.messageVersion;

                        // sanity check to avoid buffer overflow - revert back to waiting
                        if (_bytesExpected > sizeof(_buf))
                                _phase = DEC_WAIT_P1;
                }
                break;

                // receiving payload data
        case DEC_WAIT_PACKET:
                // do checksum accumulation
                _sumA += inByte;
                _sumB += _sumA;

                // store the byte
                _buf.bytes[_bytesIn++] = inByte;

                // check for complete payload received
                if (_bytesIn == _bytesExpected) {
                        _phase++;
                }
                break;

                // waiting for the checksum bytes
        case DEC_WAIT_SUM_A:
                if (inByte != _sumA) {
                        _phase = DEC_WAIT_P1;
                } else {
                        _phase++;
                }
                break;
        case DEC_WAIT_SUM_B:
                if (inByte == _sumB) {
                        // if we got this far, we have a packet
                        if (nvram.nv.packetSounds)      
                                beep.play(BEEP_PACKET);
                        // call any handler interested in this message
                        for (tableIndex = 0; 0xff != _handlerTable[tableIndex].messageID; tableIndex++)
                                if ((_handlerTable[tableIndex].messageID == MSG_ANY) ||
                                    (_handlerTable[tableIndex].messageID == _messageID))
                                        _handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, _messageID, _messageVersion, &_buf);
                } else {
                }
                _phase = DEC_WAIT_P1;
                break;
        }
}

